#include "PolyMesh.h"
#include "BBox.h"
#include <cfloat>

using namespace std;

namespace geom
{
	PolyMesh::PolyMesh()
	{
		m_baseColor[0] = m_baseColor[1] = m_baseColor[2] = 0.5;
	}

	PolyMesh::~PolyMesh()
	{
	}

	void PolyMesh::changePoint(int pos, const MeshPoint &p)
	{
		m_points[pos] = p;
	}

	void PolyMesh::setBaseColor(float r, float g, float b)
	{
		m_baseColor[0] = r;
		m_baseColor[1] = g;
		m_baseColor[2] = b;
	}

	bool PolyMesh::hasNormals() const
	{
		int numPoints = m_points.size();
		for(int i=0;i<numPoints;++i)
			if(m_points.at(i).n.module() > 0.00001)
				return true;
		return false;
	}

	void PolyMesh::addTriangle(const MeshPoint &p1, const MeshPoint &p2, const MeshPoint &p3)
	{
		Triplet<MeshPoint> t;
		t[0] = p1;
		t[1] = p2;
		t[2] = p3;

		addTriangle(t);

	}

	void PolyMesh::addTriangle(const util::Triplet<MeshPoint> &tri)
	{
		// We search for the triangle points in the points list
		Triplet<int>trisPos;
		for(int i=0;i<3;i++)
		{
			trisPos[i] = this->addPointCheckingOthers(tri[i]);	
		}
		m_tris.push_back(trisPos);	
	}

	void PolyMesh::addTriangle(const util::Triplet<int> & indicesTri)
	{
		m_tris.push_back(indicesTri);
	}

	void PolyMesh::addPoint(const MeshPoint &p)
	{
		m_points.push_back(p);
	}

	void PolyMesh::setPointNormal(int pos,const Vector3D &normal)
	{
		m_points.at(pos).n = normal;
	}

	int PolyMesh::addPointCheckingOthers(const MeshPoint &p)
	{
		int size = m_points.size();
		for(int n=0;n<size;n++)
			if(p == m_points.at(n))
				return n;

		m_points.push_back(p);
		return m_points.size()-1;
	}


	util::Triplet<MeshPoint> PolyMesh::getTriangle(int tri) const
	{
		Triplet<int> triangle = m_tris.at(tri);
		Triplet<MeshPoint> result;
		for(int i=0;i<3;i++)
			result[i] = m_points.at(triangle[i]);

		return result;
	}

	geom::BBox PolyMesh::getMeshBoundingBox() const
	{
		int numPoints = m_points.size();
		Triplet<float> minP;
		Triplet<float> maxP;
		for(int i=0;i<3;++i)
			minP[i] = FLT_MAX, maxP[i] = -FLT_MAX;

		for(int i = 0; i<numPoints;++i)
		{
			const Point3D &p = m_points[i].p;
			for(int j=0;j<3;++j)
			{
				if(p.v[j]<minP[j])
					minP[j] = p.v[j];

				if(p.v[j]>maxP[j])
					maxP[j] = p.v[j];
			}
		}

		Point3D center((maxP[0]+minP[0])/2, (maxP[1]+minP[1])/2, (maxP[2]+minP[2])/2);

		float maxSide=0;
		for(int i=0;i<3;i++)
		{
			float side = maxP[i] - center.v[i] > minP[i] - center.v[i]? maxP[i]-center.v[i] : minP[i]-center.v[i];
			if(side>maxSide)
				maxSide = side;
		}
		return geom::BBox(center,maxSide*2.01f);
	}

	FittedBBox PolyMesh::getMeshFittedBoundingBox() const
	{
		int numPoints = m_points.size();
		Point3D minP;
		Point3D maxP;
		for(int i=0;i<3;++i)
			minP.v[i] = FLT_MAX, maxP.v[i] = -FLT_MAX;

		for(int i = 0; i<numPoints;++i)
		{
			const Point3D &p = m_points[i].p;
			for(int j=0;j<3;++j)
			{
				if(p.v[j]<minP.v[j])
					minP.v[j] = p.v[j];

				if(p.v[j]>maxP.v[j])
					maxP.v[j] = p.v[j];
			}
		}

		return FittedBBox(minP,maxP);
	}

	void PolyMesh::generateNormals()
	{
		int numPoints = m_points.size();
		int numTris = m_tris.size();
		for(int i=0;i<numPoints;++i)
		{
			MeshPoint & m = m_points.at(i);
			int numNormals =0;
			Vector3D summedNormal;
			for(int j=0;j<numTris;++j)
			{
				const Triplet<int> &t = m_tris[j];
				if(t[0] == i || t[1] == i || t[2] == i)
				{
					Triangle tri(m_points[ t[0] ].p, m_points[ t[1] ].p, m_points[ t[2] ].p);
					numNormals++;
					summedNormal+=tri.getNormal();
				}
			}
			m.n = summedNormal/numNormals;

		}

	}
}
